#include "emac_internal.h"
#include "emac.h"
#include "queue.h"
#include "os_mem.h"
#include "os_io.h"
#include "sw_interrupt.h"
#include "emac_params.h"
#include "sh_mem.h"
#include "arp.h"
#include "sockets.h"

#ifdef NO_VM
static emac_tx_ctx_t tx_ctx;
#else
#error "tx_ctx is shared b/w process and ISR"
#endif

static uint8_t device_mac[6] = {0x00, 0x08, 0xEE, 0x03, 0x87, 0x6C };
static emac_params_t *emac_params = NULL;
static arp_table_t *arp_table = NULL;

static inline
void send_packet(gather_list_t *gather_list);

static inline
err_t send_arp_request(uint32_t ip_addr);

static err_t init_tx_descriptors(void)
{
    uint32_t i = 0;
    pkt_desc_t *desc_ptr = NULL;

    for (i = 0; i < NUM_TX_BUFFERS; i++)
    {
    desc_ptr = (pkt_desc_t *)(TX_DESC_MEM_BASE) + i;
    desc_ptr->next_ptr = NULL;
    desc_ptr->buf_ptr = NULL;
    desc_ptr->buf_len = 0;
    desc_ptr->packet_len = 0;
    desc_ptr->buf_offset = 0;
    desc_ptr->flags = 0;
    }

    // Set EOQ flag in the last descriptor
    // to trigger HDP addition in the first time
    desc_ptr->flags = EMAC_DSC_FLAG_EOQ;

    return ERR_SUCCESS;

}

static void register_emac_tx(emac_cfg_t *emac_cfg)
{
    raise_sw_interrupt(SW_INT_EMAC_CFG, (uint32_t)emac_cfg);
    return;
}

//#define TEST_TX
#ifdef TEST_TX
#include "process.h"
void test_emac_tx(void)
{
    uint8_t *ptr = malloc(100);
    uint8_t i = 0;
    uint16_t j = 0;
    err_t q_err;
    q_id_t tx_qid;

    q_err = lookup_queue((const uint8_t *)"emac_tx_queue", &tx_qid);

    for (j = 0; j < 300; j++) {
    ptr = malloc(100);
    for (i = 0; i < 100; i++) {
        if (i < 6) { 
        ptr[i] = 0xFF;  // form broadcast address
        } else {
        ptr[i] = i;
        }
    }
    enqueue_shared_data(tx_qid, ptr, 100);
    }

    return;
}
#endif

static inline
void free_gather_list(gather_list_t *gather_list)
{
    uint8_t i = 0;

    if (!gather_list) { return; }

    for (i = 0; i < gather_list->num_entries; i++) {
        free(gather_list->buf_list[i].buf_ptr);
    }
    free(gather_list);
}

static void
arp_timer_expired (void *arg)
{
    uint32_t i = (uint32_t)arg;
    arp_entry_t *arp_entry = &arp_table->entries[i];
    gather_list_t *gather_list = NULL;
    gather_list_t *next_gather_list = NULL;
    err_t err_code;
    bool drop_packets = FALSE;

    //printf("ARP timer expired for slot: 0x%x\n", i);

    sem_lock(&arp_entry->entry_sem);
    if (arp_entry->mac_addr_present == true) {
       // ARP just got resolved by emac-RX task.
       sem_unlock(&arp_entry->entry_sem);
       return;
    }

    if (arp_entry->arp_count < 3) {

	arp_entry->arp_timer = (user_timer_data_t *)calloc(1, sizeof(user_timer_data_t));
	if (!arp_entry->arp_timer) {
	    printf("Could not allocate arp timer. No memory!!. Dropping packets");
	    drop_packets = TRUE;
	} else {
	    arp_entry->arp_timer->interval = 1000;  // 1sec
	    arp_entry->arp_timer->periodic = false;
	    arp_entry->arp_timer->user_handler = arp_timer_expired;
	    arp_entry->arp_timer->user_handler_arg = arg;
	    arp_entry->arp_timer->q_id = emac_params->tx_queue_id;
	    // restart the timer
	    err_code = user_timer_start(arp_entry->arp_timer);
	    if (err_code != ERR_SUCCESS) {
	        printf("Failed to restart ARP timer. Dropping packets. err_code:0x%x\n", err_code);
		free((uint8_t *)arp_entry->arp_timer);
		drop_packets = TRUE;
	    } else {
	        // Send arp request again
	        send_arp_request(arp_entry->ip_address);
	        arp_entry->arp_count++;
	    }
	}
    } else {
        // No response despite arp entries.
	// Drop queued packets
	printf("ARP response not received.\n");
	drop_packets = TRUE;
    }

    if (drop_packets) {
	i = 0;

	gather_list = arp_entry->gather_list;
	while (gather_list) {
	    next_gather_list = gather_list->next;
	    free_gather_list(gather_list);
	    gather_list = next_gather_list;
	    i++;
	}
	printf("0x%x packets dropped\n", i);

	arp_entry->ip_address = 0;
	arp_entry->arp_count = 0;
	arp_entry->gather_list = NULL;
	arp_entry->arp_timer = NULL;
    }

    sem_unlock(&arp_entry->entry_sem);

    return;
}


static void expire_old_arp_entries(void *arg)
{
    uint16_t i = 0;
    arp_entry_t *arp_entry = NULL;

    for (i = 0; i < MAX_ARP_ENTRIES; i++) {

        arp_entry = (arp_entry_t *)&arp_table->entries[i];
	if (arp_entry->mac_addr_present) {
	    sem_lock(&arp_entry->entry_sem);
	    if (arp_entry->entry_recently_updated) {
	        // Wait for the next iteration and check
		// if the entry gets updating
	        arp_entry->entry_recently_updated = false;
	    } else {
	        // arp entry has expired
		arp_entry->ip_address = 0;
		arp_entry->mac_addr_present = false;		
	    }
	    sem_unlock(&arp_entry->entry_sem);
	}
    }
}

static inline
bool check_and_send_arp(gather_list_t *gather_list) {
    uint16_t i = 0;
    ip_hdr_t *ip_hdr = NULL;
    uint32_t ip_addr = 0;
    user_timer_data_t *timer_ptr = NULL;
    err_t err_code = ERR_SUCCESS;


    uint16_t empty_slot_index = 0xFFFF;

    /*
    * Check if the packet already contains a valid mac
    */
    ip_hdr = (ip_hdr_t *)gather_list->buf_list[0].buf_ptr;
    for (i = 0; i < 6; i++) {
        if (ip_hdr->dst_mac[i] != 0x00) {
	    //printf("Valid mac already filled by app for this packet\n");
	    break;   // Valid mac already filled by the app
	}
    }

    if (i != 6) {
        // valid mac already filled by app
	// pkt ready to send
	return FALSE;
    }

    // socket library filled ip address in network format. re-convert to host format
    ip_addr = ntohl(ip_hdr->dest_ip);
    //printf("ip_addr after conversion: 0x%x\n", ip_addr);

    // If ip-addr does not belong to our subnet, then we actually need the mac-addr
    // of the gateway
    if ((ip_addr & emac_params->subnet_mask) != (emac_params->ip_addr & emac_params->subnet_mask)) {
        // some other subnet
	ip_addr = emac_params->gateway_addr;
    }

    //printf("Shall be requesting arp for ip: 0x%x\n", ip_addr);

    // Perform a mac search on the arp table
    for (i = 0; i < MAX_ARP_ENTRIES; i++) {
        if (arp_table->entries[i].ip_address == 0 && empty_slot_index == 0xFFFF) {
	    empty_slot_index = i;
	} else if (arp_table->entries[i].ip_address == ip_addr) {
	    break;
	}
    }

    if (i == MAX_ARP_ENTRIES) {
        // Scanned till end. MAC entry not found
	if (empty_slot_index == 0xFFFF) {
	    // All slots filled. No room for more entries
	    printf("Insufficient space in arp table. Dropping packet");
	    free_gather_list(gather_list);
	
	} else {
	    // Request an arp
	    i = empty_slot_index;
	    //sem_lock(&arp_table->entries[i].entry_sem);
	    arp_table->entries[i].mac_addr_present = false;
	    arp_table->entries[i].gather_list = gather_list;
	    gather_list->prev = gather_list;
	    gather_list->next = NULL;
	    arp_table->entries[i].arp_count = 0;
	    
	    timer_ptr = (user_timer_data_t *)calloc(1, sizeof(user_timer_data_t));
	    err_code = ERR_SUCCESS;
	    if (!timer_ptr) {
	        err_code = ERR_NO_MEM;
	    } else {
	        arp_table->entries[i].arp_timer = timer_ptr;
	        timer_ptr->interval = 1000;  // 1 second timer
	        timer_ptr->periodic = false;
	        timer_ptr->user_handler = arp_timer_expired;
	        timer_ptr->user_handler_arg = (void *)((uint32_t)i);
	        timer_ptr->q_id = emac_params->tx_queue_id;
	    }
	   
	    if (err_code == ERR_SUCCESS) {
	        err_code = user_timer_start(timer_ptr);
		if (err_code != ERR_SUCCESS) {
		    free((uint8_t *)timer_ptr);
		    arp_table->entries[i].arp_timer = NULL;
		}
	    }

	    //printf("Starting timer with interval: 0x%x\n at index: 0x%x", timer_ptr->interval, i);
	    if (err_code != ERR_SUCCESS) {
	        // Some problem in starting the ARP timer.
		// No point in holding up the arp entry
		printf("Failed to start arp timer. err_code: 0x%x\n", err_code);
		free_gather_list(gather_list);
		arp_table->entries[i].gather_list = NULL;
	   } else {
	       // Mark the slot busy in the end
	       arp_table->entries[i].ip_address = ip_addr;
	       send_arp_request(ip_addr);
	       //printf("ARP timer started for index: 0x%x\n", i);
	   }

        }

	return TRUE;

    } else {
        // Slot containing the mac entry found
	if (arp_table->entries[i].mac_addr_present == false) {
	    // MAC not present in the entry but timer for the arp entry
	    // is already running
	    // Add gather list to the queue
	    if (!arp_table->entries[i].gather_list) {
	        printf("Error!! Expected non-null gather list while adding new to the arp-entry queue\n");
		arp_table->entries[i].gather_list = gather_list;
		gather_list->next = NULL;
		gather_list->prev = gather_list;
	    } else {
	        // Valid case
		// Add gather list to end of queue to maintain order of TX packets
		arp_table->entries[i].gather_list->prev->next = gather_list;
		gather_list->prev = arp_table->entries[i].gather_list->prev;
		gather_list->next = NULL;
		arp_table->entries[i].gather_list->prev = gather_list;
	    }
	    return TRUE;
	} else {
	    // Valid mac entry found
	    memcpy(ip_hdr->dst_mac, arp_table->entries[i].mac_addr, 6);
	    return FALSE;
	}
    }
}

static inline
err_t send_arp_request(uint32_t ip_addr)
{
    arp_pkt_t *arp_pkt = NULL;
    gather_list_t *gather_list = NULL;
    uint8_t i = 0;

    //printf("Sending arp_request for IP: 0x%x\n", ip_addr);

    arp_pkt = (arp_pkt_t *)calloc(1, sizeof(arp_pkt_t));
    if (!arp_pkt) {
        return ERR_NO_MEM;
    }

    gather_list = (gather_list_t *)calloc(1, sizeof(gather_list_t));
    if (!gather_list) {
        free(arp_pkt);
	return ERR_NO_MEM;
    }


   // dst mac
   for (i = 0; i < 6; i++) {
       arp_pkt->dst_mac[i] = 0xFF;
       arp_pkt->target_mac[i] = 0xFF;
   }

   memcpy(arp_pkt->src_mac, emac_params->mac_addr, 6);
   memcpy(arp_pkt->sender_mac, emac_params->mac_addr, 6);
   arp_pkt->eth_proto = htons(0x0806);
   arp_pkt->hw_type = htons(1); // ethernet
   arp_pkt->proto_type = htons(0x0800); // ip
   arp_pkt->hw_size = 6;
   arp_pkt->proto_size = 4;
   arp_pkt->opcode = htons(1);  // arp request
   arp_pkt->sender_ip = htonl(emac_params->ip_addr);
   arp_pkt->target_ip = htonl(ip_addr);

   gather_list->num_entries = 1;
   gather_list->buf_list[0].buf_ptr = (uint8_t *)arp_pkt;
   gather_list->buf_list[0].len = sizeof(arp_pkt_t);
   gather_list->next = gather_list->prev = NULL;

   //printf("Sending arp packet of size:0x%x\n", sizeof(arp_pkt_t));
   send_packet(gather_list);
   return ERR_SUCCESS;
}

   
static inline
void send_pending_packets(uint16_t arp_entry_index)
{
    arp_entry_t *entry = (arp_entry_t *)&arp_table->entries[arp_entry_index];
    gather_list_t *gather_list = NULL;
    gather_list_t *next_gather_list = NULL;
    
    if (entry->mac_addr_present && entry->ip_address && entry->gather_list) {
        // The pending packets have still not been wiped out by the arp timer
	//printf("Flushing pending packets for ip: 0x%x\n", entry->ip_address);
	gather_list = entry->gather_list;
	while(gather_list) {
	    next_gather_list = gather_list->next;
	    gather_list->next = gather_list->prev = NULL;
	    send_packet(gather_list);
	    gather_list = next_gather_list;
	}
	entry->gather_list = NULL;

	// stop the arp timer
	if (entry->arp_timer) {
	    user_timer_cancel(entry->arp_timer);
	    entry->arp_timer = NULL;
	}
    }

    return;
}




    

static inline
void send_packet(gather_list_t *gather_list)
{
    uint16_t tail = 0;
    pkt_desc_t *desc_ptr = NULL;
    pkt_desc_t *last_ptr = NULL;
    pkt_desc_t *first_ptr = NULL;
    pkt_desc_t *prev_desc_ptr = NULL;
    uint16_t i = 0;

    if (!gather_list) {
        printf("Invalid gather list\n");
        return;
    }

    if (check_and_send_arp(gather_list)) {
        // MAC entry was not available.
	// ARP sent
	return;
    }

    tail = tx_ctx.tail;
    desc_ptr = NULL;
    for (i = 0; i < gather_list->num_entries; i++) {

        /*
         * Now we submit the list
         */
        desc_ptr = (pkt_desc_t *)TX_DESC_MEM_BASE + ((tail + i) & (NUM_TX_BUFFERS - 1));
        desc_ptr->buf_ptr = gather_list->buf_list[i].buf_ptr;
        desc_ptr->buf_offset = 0;
        desc_ptr->buf_len = gather_list->buf_list[i].len;
        desc_ptr->flags = EMAC_DSC_FLAG_OWNER;
        desc_ptr->packet_len = 0;
    
        if (i == 0) {
            first_ptr = desc_ptr;
            desc_ptr->flags |= EMAC_DSC_FLAG_SOP;
        } else {
            prev_desc_ptr->next_ptr = desc_ptr;
        }
    
        first_ptr->packet_len += desc_ptr->buf_len;
        prev_desc_ptr = desc_ptr;
    
        //printf("desc_ptr. buf_len:0x%x, buf_ptr:0x%x", desc_ptr->buf_len, desc_ptr->buf_ptr);
	//printf("0x%x 0x%x 0x%x\n", desc_ptr->buf_ptr[0], desc_ptr->buf_ptr[6], desc_ptr->buf_ptr[12]);

    }

    tx_ctx.descriptors_remaining -= gather_list->num_entries;

    //printf("First_ptr->buf = 0x%x\n");
    //printf("Some data off first ptr: 0x%x 0x%x 0x%x 0x%x\n", first_ptr->buf_ptr[0],
    //   first_ptr->buf_ptr[1], first_ptr->buf_ptr[6], first_ptr->buf_ptr[7]);
    //printf("First ptr buf_len:0x%x, packet_len:0x%x\n", first_ptr->buf_len, first_ptr->packet_len);


    free((uint8_t *)gather_list);

    desc_ptr->flags |= EMAC_DSC_FLAG_EOP;
    desc_ptr->next_ptr = NULL;

    tx_ctx.tail = (tail + i) & (NUM_TX_BUFFERS - 1);

    tail = (tail - 1) & (NUM_TX_BUFFERS - 1);
    last_ptr = (pkt_desc_t *)TX_DESC_MEM_BASE + tail;
    last_ptr->next_ptr = first_ptr;

    if (last_ptr->flags & EMAC_DSC_FLAG_EOQ) {
        // EMAC scan is already complete.
        // Trigger again by setting HDP
        //printf("Need to HDP for TX. Flags:0x%x, HDP:0x%x\n", last_ptr->flags, TX0HDP_REG);
        TX0HDP_REG = (uint32_t)first_ptr;
    }
}

static void start_tx(queue_handle_t queue_handle)
{

    queue_msg_t q_msg;
    err_t err_code;
    uint16_t head = 0;
    uint16_t buffer_done_head = 0;
    pkt_desc_t *desc_ptr = NULL;

    gather_list_t *gather_list = NULL;
    uint16_t i = 0;

#ifdef TEST_TX
    exec_mem(test_emac_tx, 0, 6);
#endif

    while (1) {
    
        err_code = dequeue(queue_handle, &q_msg);
        if (err_code != ERR_SUCCESS) {
            printf("Error in dequeue in emac tx task: 0x%x\n", err_code);
            continue;
        }
    
        //printf("Got something to TX of size: 0x%x\n", q_msg.msg_len);
    
    
        /*
         * First we cleanup some descriptors
         */
        head = tx_ctx.head;
        buffer_done_head = tx_ctx.buffer_done_head;
    
        while (head != buffer_done_head) {
            desc_ptr = (pkt_desc_t *)TX_DESC_MEM_BASE + head;
            free(desc_ptr->buf_ptr);
            head = (head + 1) & (NUM_TX_BUFFERS - 1);
            tx_ctx.descriptors_remaining++;
        }
        tx_ctx.head = buffer_done_head;
    
        /*
         * Perform validations
         */
        if (q_msg.msg_data == NULL) { 
            printf("NULL TX data submitted to EMAC TX\n");
            continue;
        }
    
        if (q_msg.msg_len == 0) {
            printf("Invalid gather_list len submitted to EMAC "
                "TX task: 0x%x\n", q_msg.msg_len);
            continue;
        }
    
        gather_list = (gather_list_t *)q_msg.msg_data;

	if (gather_list->pending_pkt_flush) {
	    // This is a trigger from EMAC-RX task to send pending
	    // packets for which the arp entry just got resolved.

	    // In such a case, num_entries is actually the arp-table
	    // index for which the mac-addr is resolved
	    //printf("ARP entry resolved for index: 0x%x\n", gather_list->num_entries);
	    send_pending_packets(gather_list->num_entries);
	    free(gather_list);
	    continue;
	}
        //printf("Found gather-list for TX with %x entries\n", gather_list->num_entries);
        if (gather_list->num_entries > MAX_BUFFERS_PER_LIST) {
            // @todo: Increment error counter
            printf("Invalid gather list entries while TX: 0x%x. TX packet dropped.\n", 
                gather_list->num_entries);
            free(gather_list);
            continue;
        }
    
        if (gather_list->num_entries > tx_ctx.descriptors_remaining) {
            // EMAC too slow to transmit descriptors
            printf("Slow EMAC!! Dropping packets\n");
    
            for (i = 0; i < gather_list->num_entries; i++) {
                free(gather_list->buf_list[i].buf_ptr);
            }
    
            free(gather_list);
            continue;
        }
    
        send_packet(gather_list);
    }

    return;
}



    static inline
void tx_params_init(q_id_t queue_id)
{
    err_t err_code;
    void *handle;
    bool shm_created = FALSE;

    err_code = create_shared_memory(EMAC_PARAMS_MEM, SHM_CREATE_IF_NOT_FOUND, 
        sizeof(emac_params_t), &handle, (void **)&emac_params, &shm_created);
    if (err_code != ERR_SUCCESS) {
    printf("Unable to open shared memory to set TX params. err:0x%x!!\n", err_code);
    return;
    }

    if (shm_created) {
    // shared memory created for the first time
    // Initialize the locking semaphore
    err_code = sem_create2(&emac_params->emac_sem, TRUE, TRUE);
    if (err_code != ERR_SUCCESS) {
        printf("Unable to create shared memory sem in TX params!!\n");
        return;
    }
    }

    // Update emac params
    memcpy(emac_params->mac_addr, device_mac, 6);
    emac_params->ip_addr = 0xC0A8FF0c;  //192.168.255.12
    emac_params->subnet_mask = 0xFFFFFF00;
    emac_params->gateway_addr = 0xC0A8FF01;  // 192.158.255.1
    emac_params->tx_queue_id = queue_id;
    emac_params->tx_params_ready = TRUE;
    printf("EMAC TX queue id: 0x%x\n", queue_id);

    //Unlock sem
    sem_unlock(&emac_params->emac_sem);

    return;
}

static void
arp_table_init(void)
{
    err_t err_code;
    void *handle;
    bool shm_created = FALSE;
    uint16_t i = 0;
    user_timer_data_t *arp_expire_timer = NULL;

    err_code = create_shared_memory(ARP_TABLE_MEM, SHM_CREATE_IF_NOT_FOUND, 
        sizeof(arp_table_t), &handle, (void **)&arp_table, &shm_created);
    if (err_code != ERR_SUCCESS) {
    printf("Unable to open shared memory to init arp table. err:0x%x!!\n", err_code);
    return;
    }

    if (shm_created) {
        
	// Initialize arp table
        for (i = 0; i < MAX_ARP_ENTRIES; i++) {

	    memset((uint8_t *)&arp_table->entries[i], 0, sizeof(arp_entry_t));

	    err_code = sem_create2(&arp_table->entries[i].entry_sem, TRUE, TRUE);
	    if (err_code != ERR_SUCCESS) {
	        printf("Unable to initialize arp entry sem. i:0x%x, err_code:0x%x\n", i, err_code);
		return;
	    }

	    sem_unlock(&arp_table->entries[i].entry_sem);
        }
   }

   arp_expire_timer = (user_timer_data_t *)calloc(1, sizeof(user_timer_data_t));
   if (!arp_expire_timer) {
       printf("Failed to allcoate arp expire timer. ARP entries won't expire\n");
       return;
   }

   arp_expire_timer->interval = 30000;  // 30 seconds
   arp_expire_timer->periodic = true;
   arp_expire_timer->user_handler = expire_old_arp_entries;
   arp_expire_timer->user_handler_arg = NULL;
   arp_expire_timer->q_id = emac_params->tx_queue_id;
   err_code = user_timer_start(arp_expire_timer);
   if (err_code != ERR_SUCCESS) {
       printf("Failed to start arp expire timer. ARP entries won't expire!!\n");
       free(arp_expire_timer);
       return;
   }

   return;

}

void emac_txfun(void)
{
    err_t err_code;
    queue_handle_t q_handle;
    emac_cfg_t emac_cfg;

    err_t ret_val = ERR_SUCCESS;
    ret_val = init_tx_descriptors();
    if (ret_val != ERR_SUCCESS) {
        return;
    }

    tx_ctx.head = 0;
    tx_ctx.tail = 0;
    tx_ctx.buffer_done_head = 0;
    tx_ctx.descriptors_remaining = NUM_TX_BUFFERS;

    memset((uint8_t *)&emac_cfg, 0, sizeof(emac_cfg_t));
    emac_cfg.tx_ctx = &tx_ctx;
    register_emac_tx(&emac_cfg);

    err_code = create_queue((const uint8_t *)"emac_tx_queue", NULL, &q_handle);
    if (err_code != ERR_SUCCESS) {
        printf("EMAC TX task queue creation failed!!\n");
        return;
    }

    tx_params_init(q_handle->queue_id);
    arp_table_init();

    /*
     * Now we are ready to activate the interrupts
     */
    TXCONTROL_REG = 1;
    MACCONTROL_REG = MACCONTROL_REG | (0x1 << 5);  // enable GMIIEN

    // Enable TX module interrupts
    C0TXEN_REG = 1;
    //C1RXEN_REG = 1;
    C0MISCEN_REG = 0x4;
    //C1MISCEN_REG = 0x4;

    start_tx(q_handle);

}
